//
// File: leg1Ang.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 12-Apr-2025 20:50:42
//

// Include Files
#include "leg1Ang.h"
#include <cmath>

// Function Definitions
//
// 毫米做单位
// L1=54;
//
// Arguments    : double X
//                double Y
//                double Z
//                double *L1S1
//                double *L1S2
//                double *L1S3
// Return Type  : void
//
void leg1Ang(double X, double Y, double Z, double *L1S1, double *L1S2,
             double *L1S3)
{
  double A3;
  double Lyz;
  double ang_1yz;
  //
  // A1
  Lyz = std::sqrt(Y * Y + Z * Z);
  ang_1yz = std::acos(73.5 / Lyz);
  if (Z < 0.0) {
    *L1S1 = -std::acos(Y / Lyz) + ang_1yz;
  } else {
    *L1S1 = std::acos(Y / Lyz) - ang_1yz;
  }
  //
  // A3
  if (Z < 0.0) {
    ang_1yz = -std::sqrt(Lyz * Lyz - 5402.25);
  } else {
    ang_1yz = std::sqrt(Lyz * Lyz - 5402.25);
  }
  //  确保 c3 在 [-1, 1] 范围内
  Lyz = X * X;
  A3 = std::acos(
      std::fmax(std::fmin((((Lyz + ang_1yz * ang_1yz) - 22500.0) - 10625.0) /
                              30923.292192132456,
                          1.0),
                -1.0));
  // Ag1(3)=A3-Ang_thre;
  //
  // A2
  ang_1yz = std::sqrt(ang_1yz * ang_1yz + Lyz);
  if (Z < 0.0) {
    Lyz = -std::acos(X / ang_1yz);
  } else {
    Lyz = std::acos(X / ang_1yz);
  }
  // c23=(Lxz_x*cos(aph)-L3*cos(A3))/L2;
  //  确保 c23 在 [-1, 1] 范围内
  //      if(Z<0)
  //          A23=-acos(c23);
  //      else
  //          A23=acos(c23);
  //      end
  // L1S2=A23;
  // A23=acos(c23);
  //
  // 转换舵机参数
  *L1S1 += 1.5707963267948966;
  *L1S1 = *L1S1 * 180.0 / 3.1415926535897931;
  *L1S1 = std::trunc(*L1S1 * 1000.0) / 1000.0;
  *L1S2 =
      std::trunc(
          -(Lyz -
            std::acos(std::fmax(
                std::fmin((103.07764064044152 * std::cos(A3) + 150.0) / ang_1yz,
                          1.0),
                -1.0))) *
          180.0 / 3.1415926535897931 * 1000.0) /
      1000.0;
  *L1S3 = std::trunc(((A3 - 1.3258176636680326) + 1.5707963267948966) * 180.0 /
                     3.1415926535897931 * 1000.0) /
          1000.0;
}

//
// File trailer for leg1Ang.cpp
//
// [EOF]
//
